﻿using System.Collections.Generic;
using Duality;
using OpenTK;

namespace ClientApp.Avoidance
{
	public class SingleVO
	{
		private Agent other;
		protected Vector2 offset;
		protected Vector2 left;
		protected Vector2 right;

		public SingleVO(Agent self, Agent other)
		{
			this.other = other;
			// this comes from reciprocal velocity obstacles
			float thisFactor = other.GameObj.Transform.Vel.Xy.Length / (self.GameObj.Transform.Vel.Xy.Length + other.GameObj.Transform.Vel.Xy.Length);
			if (float.IsNaN(thisFactor))
				thisFactor = 1.0f;
			offset = thisFactor * self.GameObj.Transform.Vel.Xy + (1.0f - thisFactor) * other.GameObj.Transform.Vel.Xy;
			//offset = other.GameObj.Transform.Vel.Xy;

			// standard velocity obstacles
			float minkowskiSumRadius = self.ScaledRadius + other.ScaledRadius;
			Vector2 direction = other.GameObj.Transform.Pos.Xy - self.GameObj.Transform.Pos.Xy;
			float directionAngle = MathF.Atan2(direction.Y, direction.X);
			float deltaAngle = MathF.Asin(minkowskiSumRadius / direction.LengthFast);
			if (float.IsNaN(deltaAngle)) // this happens due rounding errors near 90°
				deltaAngle = MathF.PiOver2;
			left = new Vector2(MathF.Cos(directionAngle - deltaAngle), MathF.Sin(directionAngle - deltaAngle));
			right = new Vector2(MathF.Cos(directionAngle + deltaAngle), MathF.Sin(directionAngle + deltaAngle));
			// this comes from hybrid reciprocal velocity obstacles
			// will avoid oscillation
			offset -= right * self.GameObj.Transform.Vel.Xy.LengthFast * 0.1f;
		}

		public bool intersect(SingleVO otherVO, ICollection<Vector2> resultCollection)
		{
			bool result = true;
			Vector2 relOtherPosition = otherVO.Offset - offset;
			result &= intersect(relOtherPosition, left, otherVO.Left, resultCollection);
			result &= intersect(relOtherPosition, left, otherVO.Right, resultCollection);
			result &= intersect(relOtherPosition, right, otherVO.Left, resultCollection);
			result &= intersect(relOtherPosition, right, otherVO.Right, resultCollection);
			return result;
		}

		private bool intersect(Vector2 relOtherPosition, Vector2 selfVec, Vector2 otherVec, ICollection<Vector2> resultCollection)
		{
			float selfCrossOther = cross2D(selfVec, otherVec);
			if (selfCrossOther == 0.0f)
				return false;

			float t = (relOtherPosition.X * otherVec.Y - relOtherPosition.Y * otherVec.X) / selfCrossOther;
			if (t < 0.0f)
				return false;

			float u = (relOtherPosition.X * selfVec.Y - relOtherPosition.Y * selfVec.X) / selfCrossOther;
			if (u < 0.0f)
				return false;

			resultCollection.Add(offset + t * selfVec);
			return true;
		}

		public bool project(Vector2 velocity, ICollection<Vector2> resultCollection)
		{
			Vector2 relVelocity = velocity - offset;
			float leftFactor = Vector2.Dot(left, relVelocity) / left.LengthSquared;
			if (leftFactor > 0.0f)
				resultCollection.Add(offset + left * leftFactor);
			float rightFactor = Vector2.Dot(right, relVelocity) / right.LengthSquared;
			if (rightFactor > 0.0f)
				resultCollection.Add(offset + right * rightFactor);
			return false;
		}

		internal bool containsVeloctiy(Vector2 velocity)
		{
			Vector2 relVelocity = velocity - offset;
			return cross2D(relVelocity, left) < -0.001f && cross2D(relVelocity, right) > 0.001f;
		}

		internal float cross2D(Vector2 a, Vector2 b)
		{
			return a.X * b.Y - a.Y * b.X;
		}

		public OpenTK.Vector2 Offset
		{
			get { return offset; }
		}

		public OpenTK.Vector2 Left
		{
			get { return left; }
		}

		public OpenTK.Vector2 Right
		{
			get { return right; }
		}

		public Agent Other
		{
			get { return other; }
		}
	}
}